package codechicken.wirelessredstone.core;

public class WRMat4
{
	public WRMat4()
	{
		loadIdentity();
	}
	
	public WRMat4 loadIdentity()
	{
		mat = new float[16];
		mat[0] = mat[5] = mat[10] = mat[15] = 1;
		return this;
	}

	public WRVector3 translate(WRVector3 vec)
	{
		float x = vec.x*mat[0]+vec.y*mat[1]+vec.z*mat[2]+mat[3];
		float y = vec.x*mat[4]+vec.y*mat[5]+vec.z*mat[6]+mat[7];
		float z = vec.x*mat[8]+vec.y*mat[9]+vec.z*mat[10]+mat[11];
		
		vec.x = x;
		vec.y = y;
		vec.z = z;
		
		return vec;
	}
	
	public static WRMat4 rotationMat(double angle, WRVector3 axis)
	{
		axis = axis.copy().normalize();
		float x = axis.x;
		float y = axis.y;
		float z = axis.z;
		
		angle *= 0.0174532925;//toRad
		float cos = (float) Math.cos(angle);
		float ocos = 1 - cos;
		float sin = (float) Math.sin(angle);
		
		WRMat4 rotmat = new WRMat4();
		rotmat.mat[0] = x*x*ocos+cos;
		rotmat.mat[1] = y*x*ocos+z*sin;
		rotmat.mat[2] = x*z*ocos-y*sin;
		rotmat.mat[4] = x*y*ocos-z*sin;
		rotmat.mat[5] = y*y*ocos+cos;
		rotmat.mat[6] = y*z*ocos+x*sin;
		rotmat.mat[8] = x*z*ocos+y*sin;
		rotmat.mat[9] = y*z*ocos-x*sin;
		rotmat.mat[10] = z*z*ocos+cos;
		rotmat.mat[15] = 1;
		
		return rotmat;
	}
	
	float[] mat;
}
